package simulation;

import java.io.IOException;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.Map;
import java.util.Timer;
import java.util.TimerTask;
import java.util.concurrent.Semaphore;

import maze.Location;
import maze.Maze;
import pacman.Ghost;
import pacman.Pacman;

public class Simulator {
	private Maze maze;
	private ColorMap colormap;
	private ArrayList<Ghost> ghosts = new ArrayList<Ghost>();
	private Map<Ghost, Location> ghostLocations;
	private Map<Ghost, Integer> ghostRotations; // degrees
	private Map<Ghost, Semaphore> ghostLocks;
	private Map<Ghost, Semaphore> ghostTravelLocks;

	private Pacman pacman;

	public float speed = 350; // wheel-turnspeed of the ghosts, degree/s MUST BE
								// THE SAME AS THE SPEED OF THE REAL ROBOT!!
	private float wheelCirc = 15; // circumference of a wheel, cm MUST BE THE
									// SAME AS THE CIRC OF THE REAL ROBOT!!
	private float wheelBase = 15; // distance between robot wheels, cm MUST BE
									// THE SAME AS THE BASE OF THE REAL ROBOT!!

	int period = 10;

	public Simulator(Maze m, ColorMap c) {
		maze = m;
		colormap = c;
		ghostLocations = new HashMap<Ghost, Location>();
		ghostRotations = new HashMap<Ghost, Integer>();
		ghostLocks = new HashMap<Ghost, Semaphore>();

	}

	public Maze getMaze() {
		return maze;
	}

	public ColorMap getColorMap() {
		return colormap;
	}

	public Map<Ghost, Location> getGhostLocations() {
		return ghostLocations;
	}

	public Map<Ghost, Integer> getGhostRotations() {
		return ghostRotations;
	}

	public Pacman getPacman() {
		return pacman;
	}

	public void setPacman(Pacman pacman) {
		this.pacman = pacman;
	}

	public void addGhost(Ghost ghost, Location loc, int rotation) {
		ghosts.add(ghost);
		ghostLocations.put(ghost, loc);
		ghostRotations.put(ghost, rotation);
		ghostLocks.put(ghost, new Semaphore(1));
		try {
			ghostLocks.get(ghost).acquire();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		ghost.setSimulator(this);
	}

	public int SimulateLightSensor(Ghost ghost) {
		Location loc = ghostLocations.get(ghost);
		return colormap.GetSensorReading((int) (loc.x + 0.5f), (int) (loc.y + 0.5f));
	}

	public int SimulateUltrasonicSensor(Ghost ghost)
	{
		int distToWall =  maze.getMaxDistance(ghostLocations.get(ghost), ghostRotations.get(ghost)+ghost.getSensorRotation());
		int distToRobot = UltrasonicRobotCheck(ghost,20);
		//return Math.min(distToWall, distToRobot);
		return distToWall;
	}

	/**
	 * checks if there is an other ghost between him and the closest wall and returns the distance;
	 * 
	 * @param sensorWidth
	 * 			| the angle of effect that the sensor can see
	 * @return
	 * 		|the minimum off the distance between him and the closest wall and him and the closest robot.
	 */
	private int UltrasonicRobotCheck(Ghost ghost,int sensorWidth){
		int closest = Integer.MAX_VALUE;
		Location location = ghostLocations.get(ghost);
		int sensorangle = ghostRotations.get(ghost)+ghost.getSensorRotation();
		for (Ghost other : ghostLocations.keySet()) {
			if(!other.equals(ghost)){
				Location locOther = ghostLocations.get(other);
				int angle;
				
				
					angle = (int) Math.toDegrees(Math.atan2((locOther.y-location.y),(locOther.x-location.x)));
				
				
				//check if angle is in the sensorrange
				if(Math.abs(angle-sensorangle) <= sensorWidth){
					//to avoid division by zero
					
					if (angle == 0 || angle == 180 || angle == -180)
						angle += 1;
					
					
					int dist = (int) Math.abs(((locOther.y-location.y)/Math.sin(Math.toRadians(angle))));
					if (dist<closest){
						closest = dist;
						
					}
					
				}
				
				
			}
		}
		return closest;
		
	}
	
	/**
	 * De direction wordt genummerd van links naar rechts.  Er zijn 9 mogelijke sectoren waarin pacman
	 * zich kan bevinden, gaande van 120° tot -120°
	 */
	public int SimulateInfraredSensor(Ghost ghost) {
		// beperking op de afstand die de sensor kan zien...
		int maxDistanceSensor = 60;

		/*
		 * rotatie 0 is naar rechts kijken 1 2 3 4 0 Jos 5 6 9 8 7
		 */
		Location pacmanLocation = getPacman().getLocation();
		// verplaatsingsvector
		Location ghostLocation = ghostLocations.get(ghost);
		// double rico = (pacmanLocation.getY() -
		// ghostLocation.getY())/(pacmanLocation.getX() - ghostLocation.getX());
		// double angle = Math.atan(rico)*(180/Math.PI) +
		// getGhost().getRotation();
		// // bepalen we de sector waarin pacman zich bevind voor de
		// infraroodsensor
		// double sectorDouble = (-angle+120)/26.66667;
		// int sector = (int) (Math.ceil(sectorDouble));
		// //Vanaf hier proberen we de maxdistance uit te voeren en te
		// vergelijken met de afstand dat pacman echt is
		// // ten opzichte van de robot: dit op basis van afstandsberekening
		// tussen 2 punten
		// int IntAngle = (int) angle;
		// double SqrtXDist =
		// (pacmanLocation.getX()-ghostLocation.getX())*(pacmanLocation.getX()-ghostLocation.getX());
		// double SqrtYDist =
		// (pacmanLocation.getY()-ghostLocation.getY())*(pacmanLocation.getY()-ghostLocation.getY());
		// int distanceCalculated = (int) Math.sqrt(SqrtXDist + SqrtYDist);
		// int maxDistance = getGhost().getMaze().getMaxDistance(ghostLocation,
		// IntAngle);
		// if( maxDistance < distanceCalculated){
		// sector = 0;
		// }
		//
		// return sector;
		double diffx = pacmanLocation.x - ghostLocation.x;
		double diffy = pacmanLocation.y - ghostLocation.y;
		// verplaatsingsvector draaien (zoek op: "2d rotation matrix) om te
		// compenseren voor draaiing robot
		double counterAngle = -ghostRotations.get(ghost) * Math.PI / 180.0; // in
																			// radialen
		double temp = Math.cos(counterAngle) * diffx - Math.sin(counterAngle) * diffy;
		diffy = Math.sin(counterAngle) * diffx + Math.cos(counterAngle) * diffy;
		diffx = temp;
		// afstand uitrekenenen en bewaren, verplaatsingsvector normaliseren
		// (behoud de richting)
		double distance = Math.sqrt(diffx * diffx + diffy * diffy);
		diffx /= distance;
		diffy /= distance;
		int direction = 0;
		double dirWidth = 240.0 / 9.0;
		// bepalen van de richting
		if (diffx < Math.cos(120.0 * Math.PI / 180.0)) { // ~120
			direction = 0;
		} else if (diffx < Math.cos((120.0 - dirWidth) * Math.PI / 180.0)) { // ~84
			direction = (diffy > 0.0) ? 1 : 9;
		} else if (diffx < Math.cos((120.0 - dirWidth * 2) * Math.PI / 180.0)) { // ~58
			direction = (diffy > 0.0) ? 2 : 8;
		} else if (diffx < Math.cos((120.0 - dirWidth * 3) * Math.PI / 180.0)) { // ~31
			direction = (diffy > 0.0) ? 3 : 7;
		} else if (diffy > Math.sin((120.0 - dirWidth * 4) * Math.PI / 180.0)) { // overschakelen
																					// op
																					// sinus
																					// voor
																					// nauwkeurigheid
			direction = 4;
		} else if (diffy < Math.sin((120.0 - dirWidth * 5) * Math.PI / 180.0)) { // overschakelen
																					// op
																					// sinus
																					// voor
																					// nauwkeurigheid
			direction = 6;
		} else
			direction = 5;
		double angle = ((Math.abs(direction - 5) >= 3) ? Math.acos(diffx) : Math.asin(diffy)) * 180 / Math.PI; // calculate
																												// angle,
																												// kies
																												// cos
																												// of
																												// sin
																												// voor
																												// beste
																												// nauwkeurigheid
		if (Math.abs(direction - 5) >= 3) {
			if (Math.asin(diffy) < 0.0)
				angle = -angle;
		} else {
			if (Math.acos(diffx) < 0.0)
				angle = 180.0 - angle;
		}
		int maxDistance = maze.getMaxDistance(ghostLocation, (int) angle + ghostRotations.get(ghost));
		if (maxDistance < distance) {
			direction = 0;
		}
		return direction;
	}

	/**
	 * Simulates the traveling of a robot.
	 * 
	 * @param ghost
	 * @param distance
	 *            cm
	 * @throws IOException
	 */
	public void SimulateTravel(Ghost ghost, int distance) throws IOException {
		/*
		 * try { ghostLocks.get(ghost).acquire();//this will block if another
		 * rotate/travel task is busy } catch(InterruptedException ie) { throw
		 * new RuntimeException("The Simulator thread was interrupted."); }
		 */
		while (ghost.isTravelling)
			;
		Timer timer = new Timer();
		int period = 10; // milliseconds SHOULD BE SMALL ENOUGH (otherwise,
							// lightsensor will not read barcodes accurately)
		float travelSpeed = speed * wheelCirc / 360;

		TravelTask travelTask = new TravelTask(ghost, distance, travelSpeed, period);
		timer.schedule(travelTask, 0, period);
		try {
			ghostLocks.get(ghost).acquire();// this blocks until the task is
											// done
		} catch (InterruptedException ie) {
			throw new RuntimeException("The Simulator thread was interrupted.");
		}

		ghost.sendPOSITION();
	}

	/**
	 * Simulates the rotation (in place) of a robot.
	 * 
	 * @param ghost
	 * @param angle
	 *            degr
	 */
	public void SimulateRotation(Ghost ghost, int angle) {
		/*
		 * try { ghostLocks.get(ghost).acquire(); } catch(InterruptedException
		 * ie) { throw new
		 * RuntimeException("The Simulator thread was interrupted."); }
		 */
		while (ghost.isTravelling)
			;
		Timer timer = new Timer();
		int period = 10; // milliseconds
		float turnSpeed = (float) (((wheelCirc) * (speed)) / ((Math.PI) * (wheelBase)));

		RotateTask rotateTask = new RotateTask(ghost, angle, turnSpeed, period);
		timer.schedule(rotateTask, 0, period);
		try {
			ghostLocks.get(ghost).acquire();
		} catch (InterruptedException ie) {
			throw new RuntimeException("The Simulator thread was interrupted.");
		}
	}

	class TravelTask extends TimerTask {

		private Ghost ghost;
		private float dist;
		private double startX;
		private double startY;
		private int rot;
		private int remainingTicks;
		private int totalTicks;
		private float distancePerTick;

		/**
		 * Constructor
		 * 
		 * @param g
		 * @param distance
		 *            cm
		 * @param travelspeed
		 *            cm/s
		 * @param updateIntervalMilli
		 *            millisec
		 */
		public TravelTask(Ghost g, float distance, float travelspeed, float updateIntervalMilli) {
			ghost = g;
			startX = ghostLocations.get(ghost).x;
			startY = ghostLocations.get(ghost).y;
			rot = ghostRotations.get(ghost);
			dist = distance;

			distancePerTick = travelspeed * updateIntervalMilli / 1000;
			totalTicks = Math.abs((int) (distance / distancePerTick)); // =
																		// totaltime/timepertick
			remainingTicks = totalTicks;
		}

		@Override
		public void run() {
			ghost.isTravelling = true;
			rot = ghostRotations.get(ghost);
			if (remainingTicks >= 0) {

				int neg = 1; // -1 if negative distance +1 if positive
				if (dist < 0)
					neg = -1;
				Location prev = ghostLocations.get(ghost);
				double newX;
				double newY;
				double rotRad = Math.toRadians(rot);
				newX = prev.x + neg * (Math.cos(rotRad) * distancePerTick);
				newY = prev.y + neg * (Math.sin(rotRad) * distancePerTick);

				Location newLoc = new Location(newX, newY);
				ghostLocations.put(ghost, newLoc);

				if (dist < 0 && (float) maze.getMaxDistance(ghostLocations.get(ghost), rot + 180) <= 2 * distancePerTick) {

					rotRad = Math.toRadians(rot);
					newX = newX + neg * (Math.cos(rotRad) * (float) (maze.getMaxDistance(ghostLocations.get(ghost), rot + 180)) - 0.1);
					newY = newY + neg * (Math.sin(rotRad) * (float) (maze.getMaxDistance(ghostLocations.get(ghost), rot + 180)) - 0.1);

					newLoc = new Location(newX, newY);
					ghostLocations.put(ghost, newLoc);
					ghostLocks.get(ghost).release();
					ghost.isTravelling = false;
					this.cancel();
				}
				if (dist > 0 && (float) maze.getMaxDistance(ghostLocations.get(ghost), rot) <= 2 * distancePerTick) {
					rotRad = Math.toRadians(rot);
					newX = newX + neg * (Math.cos(rotRad) * (float) (maze.getMaxDistance(ghostLocations.get(ghost), rot)) - 0.1);
					newY = newY + neg * (Math.sin(rotRad) * (float) (maze.getMaxDistance(ghostLocations.get(ghost), rot)) - 0.1);

					newLoc = new Location(newX, newY);
					ghostLocations.put(ghost, newLoc);
					ghostLocks.get(ghost).release();
					ghost.isTravelling = false;
					this.cancel();
				}
				remainingTicks--;
			} else {
				/*
				 * int neg = 1; // -1 if negative distance +1 if positive if
				 * (dist<0) neg = -1; //set final pos Location prev =
				 * ghostLocations.get(ghost); double newX; double newY; double
				 * rotRad = Math.toRadians(rot); newX = prev.x +
				 * neg*(Math.cos(rotRad
				 * )*distancePerTick);//(float)(maze.getMaxDistance
				 * (ghostLocations.get(ghost), rot))-0.1); newY = prev.x +
				 * neg*(Math
				 * .sin(rotRad)*distancePerTick);//(float)(maze.getMaxDistance
				 * (ghostLocations.get(ghost), rot))-0.1);
				 * 
				 * Location newLoc = new Location(newX, newY);
				 * ghostLocations.put(ghost, newLoc);
				 */
				ghost.isTravelling = false;
				ghostLocks.get(ghost).release();
				this.cancel();
			}

		}
	}

	class RotateTask extends TimerTask {

		private Ghost ghost;
		private int angle;
		private int startAngle;
		private int remainingTicks;
		private int totalTicks;
		private float degreesPerTick;

		/**
		 * Constructor
		 * 
		 * @param g
		 * @param angle
		 *            degrees to turn CCW
		 * @param turnspeed
		 *            degrees/s
		 * @param updateIntervalMilli
		 *            milliseconds
		 */
		public RotateTask(Ghost g, int angle, double turnspeed, int updateIntervalMilli) {
			ghost = g;
			startAngle = ghostRotations.get(ghost);
			this.angle = angle;

			degreesPerTick = (float) (turnspeed * (float) (updateIntervalMilli) / 1000.0);
			totalTicks = (int) (Math.abs(angle) / degreesPerTick);
			remainingTicks = totalTicks;
		}

		@Override
		public void run() {
			ghost.isTravelling = true;
			if (remainingTicks > 0) {
				int newAngle = (int) (startAngle + Math.signum(angle) * degreesPerTick * (totalTicks - remainingTicks));
				ghostRotations.put(ghost, newAngle % 360);

				remainingTicks--;
			} else {
				ghostRotations.put(ghost, (startAngle + angle) % 360);
				ghost.isTravelling = false;
				ghostLocks.get(ghost).release();
				this.cancel();
			}

		}

	}
}
